#include "AStar.h"
#include <iostream>

#include <math.h>
AStar::AStar()
{
}


AStar::~AStar()
{
}


std::vector<Node*> AStar::Setup(Node* p_StartingNode, Node* p_GoalNode)
{

	m_vPath.clear();
	m_StartingNode = p_StartingNode;
	m_GoalNode = p_GoalNode;

	m_OpenList.push_back(m_StartingNode);
	tempNode = nullptr;
	tempNodeID = 0;
	tempNode2 = m_OpenList[0];

	found = false;
	Run();

	Reset();

	// Return the path to the target
	return m_vPath;
}

void AStar::Run()
{
	do
	{
		//	Add current square(A) to open list.
		//	Look around you, add all walkabkle squares to the open list(N-0, S-1, W-2, E-3, NE-4, SE-5, SW-6, NW-7)
		//	For each of these, add square(A) as it's parent.

		for (int i = 0; i < tempNode2->m_xAdjacentNodes.size(); i++)
		{
			if (nullptr == tempNode2->m_xAdjacentNodes[i])
			{
				continue;
			}
			tempNode = tempNode2->m_xAdjacentNodes[i];

			if (tempNode != nullptr && tempNode->m_bBlocked != true && tempNode->m_bIsOnOpenList == false && tempNode->m_bIsOnClosedList == false)
			{

				// Stop the agent from walking diagonally if the agent would be walking into a blocked node.
				switch (i)
				{
					case 4:
						if ((tempNode2->m_xAdjacentNodes[0]->m_bBlocked) || (tempNode2->m_xAdjacentNodes[3]->m_bBlocked))
						{
							continue;
						}
						break;

					case 5:
						if ((tempNode2->m_xAdjacentNodes[3]->m_bBlocked) || (tempNode2->m_xAdjacentNodes[1]->m_bBlocked))
						{
							continue;
						}
						break;

					case 6:
						if ((tempNode2->m_xAdjacentNodes[1]->m_bBlocked) || (tempNode2->m_xAdjacentNodes[2]->m_bBlocked))
						{
							continue;
						}
						break;

					case 7:
						if ((tempNode2->m_xAdjacentNodes[2]->m_bBlocked) || (tempNode2->m_xAdjacentNodes[0]->m_bBlocked))
						{
							continue;
						}
						break;
				}

				m_OpenList.push_back(tempNode);
				tempNode->m_ParentNode = tempNode2;
				if (i > 3)
				{
					tempNode->m_iGCost = tempNode->m_ParentNode->m_iGCost + 14;
				}
				else
				{
					tempNode->m_iGCost = tempNode->m_ParentNode->m_iGCost + 10;
				}

				tempNode->m_iHCost = CalculateMDistance(tempNode);
				tempNode->m_iFCost = tempNode->m_iGCost + tempNode->m_iHCost;
				//------------tempNode->m_xRect->setFillColor(sf::Color::Red);
				tempNode->m_bIsOnOpenList = true;
			}
		}
		
		// Remove A from open list, add to closed list.
		m_ClosedList.push_back(tempNode2);
		tempNode2->m_bIsOnOpenList = false;
		tempNode2->m_bIsOnClosedList = true;

		// ---------------- CRASH HERE --------------------//
		// Vector subscript out of range, semi-rare crash, openlist seem to be empty when crash occurs
		// OpenList should not be able to be empty at this point, as it will crash.
		m_OpenList.erase(m_OpenList.begin() + tempNodeID);
		
		//	Choose square on open list with smallest F value, Repeat.
		int	m_LowestCost = 5000;
		if (m_OpenList.size() > 0)
		{
			m_LowestCost = m_OpenList.back()->m_iFCost + 1;
		}
		//-------------------------------------------------//
		
		for (int i = 0; i < m_OpenList.size(); i++)
		{
			if (m_LowestCost > m_OpenList[i]->m_iFCost)
			{

				if (m_OpenList[i]->m_bBlocked == false)
				{
					
					m_LowestCost = m_OpenList[i]->m_iFCost;
					tempNode2 = m_OpenList[i];
					//---------------tempNode2->m_xRect->setFillColor(sf::Color::Yellow);
					tempNodeID = i;
				}
				
			}
		}

		if (tempNode2 == m_GoalNode)
		{
			found = true;
			//std::cout << "HOOOORAY" << "\n";
			while  (tempNode2->m_ParentNode != nullptr)
			{
				//-------------tempNode2->m_xRect->setFillColor(sf::Color::Blue);
				m_vPath.push_back(tempNode2);
				tempNode2 = tempNode2->m_ParentNode;
			}
		}

		//m_StartingNode->m_xRect->setFillColor(sf::Color::Magenta);

		//m_GoalNode->m_xRect->setFillColor(sf::Color::White);
	
		

	} 
	while (found == false);


}


int AStar::CalculateMDistance(Node* p_Node)
{

	int m_iXDiffrence = abs(p_Node->m_vID.x - m_GoalNode->m_vID.x);
	int m_iYDiffrence = abs(p_Node->m_vID.y - m_GoalNode->m_vID.y);
	
	int m_iTotal = m_iYDiffrence + m_iXDiffrence;
	m_iTotal *= 11;
	//std::cout << m_iTotal << "\n";
	return m_iTotal;
}
int AStar::CalculateMDistance(Node* p_Node1, Node* p_Node2 )
{

	int m_iXDiffrence = abs(p_Node1->m_vID.x - p_Node2->m_vID.x);
	int m_iYDiffrence = abs(p_Node1->m_vID.y - p_Node2->m_vID.y);

	int m_iTotal = m_iYDiffrence + m_iXDiffrence;
	m_iTotal *= 11;
	//std::cout << m_iTotal << "\n";
	return m_iTotal;
}

void AStar::Reset()
{
	for (int i = 0; i < m_OpenList.size(); i++)
	{
		m_OpenList[i]->m_bIsOnClosedList = false;
		m_OpenList[i]->m_bIsOnOpenList = false;
		m_OpenList[i]->m_ParentNode = nullptr;
		m_OpenList[i]->m_iFCost = 0;
		m_OpenList[i]->m_iGCost = 0;
		m_OpenList[i]->m_iHCost = 0;
	}
	m_OpenList.clear();

	for (int i = 0; i < m_ClosedList.size(); i++)
	{
		m_ClosedList[i]->m_bIsOnClosedList = false;
		m_ClosedList[i]->m_bIsOnOpenList = false;
		m_ClosedList[i]->m_ParentNode = nullptr;
		m_ClosedList[i]->m_iFCost = 0;
		m_ClosedList[i]->m_iGCost = 0;
		m_ClosedList[i]->m_iHCost = 0;
	}
	m_ClosedList.clear();
}